/*
 * demo.cpp
 *
 *  Created on: Nov 26, 2012
 *      Author: michiel
 */

#include "model/demo.h"

void DemoModel::init() {
	ears_pub = nh.advertise<std_msgs::Int32>(Topic::EMO_HANDLER_EARS, 1);
	eyes_pub = nh.advertise<comm::eyes>(Topic::EMO_HANDLER_EYES, 1);
	trunk_pub = nh.advertise<std_msgs::Int32>(Topic::EMO_HANDLER_TRUNK, 1);
	eyebrows_pub = nh.advertise<std_msgs::Int32>(Topic::EMO_HANDLER_BROW, 1);
}

void DemoModel::spin() {
	ros::Rate loop_rate(0.25);

	while (ros::ok()) {
		ears_state = flip(ears_state);
		trunk_state = flip(trunk_state);
		brows_state = flip(brows_state);

		std_msgs::Int32 msg_ears;
		std_msgs::Int32 msg_brows;
		std_msgs::Int32 msg_trunk;
		comm::eyes msg_eyes;

		msg_ears.data				= ears_state;
		msg_brows.data			= brows_state;
		msg_trunk.data			= trunk_state;

		msg_eyes.leftpitch	= eyes_pitch_state;
		msg_eyes.leftyaw		= eyes_yaw_state;
		msg_eyes.rightpitch	= eyes_pitch_state;
		msg_eyes.rightyaw		= eyes_yaw_state;

		ears_pub.publish(msg_ears);
		eyebrows_pub.publish(msg_brows);
		trunk_pub.publish(msg_trunk);
		eyes_pub.publish(msg_eyes);

		ros::spinOnce();
		loop_rate.sleep();
	}
}



int main(int argc, char** argv) {
	ros::init(argc, argv, "model_demo");

	DemoModel demo;
	demo.init();
	demo.spin();
}


